;***************************************************************************
;		import important registers and value
;***************************************************************************
SRCPND		EQU		0x4a000000	;source pending
INTPND		EQU		0x4a000010	;interrupt request status

EINTPEND	EQU		0x560000a8	;external interrupt pending
INTOFFSET	EQU		0x4a000014

USERMODE	EQU		0x10
FIQMODE	EQU		0x11
IRQMODE	EQU		0x12
SVCMODE	EQU		0x13
ABORTMODE	EQU		0x17
UNDEFMODE	EQU		0x1b
MODEMASK	EQU		0x1f
NOINT		EQU		0xc0
;*************************************************************************
;		FLAGS IMPORT & EXPORT
;*************************************************************************
	IMPORT OSRunning
	IMPORT OSTCBCur
	IMPORT OSTCBHighRdy
	IMPORT OSPrioCur
	IMPORT OSPrioHighRdy
	IMPORT OSIntNesting

	IMPORT OSIntEnter
	IMPORT OSIntExit
	IMPORT OSTaskSwHook
	IMPORT OSTimeTick

	IMPORT ledon
	IMPORT ledoff

;	IMPORT HandleEINT0
	IMPORT irq_process

	EXPORT OSStartHighRdy
	EXPORT OSCtxSw
	EXPORT OSIntCtxSw
	EXPORT OSTickISR

	EXPORT OS_CPU_SR_Save
	EXPORT OS_CPU_SR_Restore


	EXPORT OS_CPU_IRQ_ISR

	PRESERVE8
	AREA UCOS_ARM, CODE, READONLY
;//*********************************************************************************************************
;//                                          START MULTITASKING
;//                                       void OSStartHighRdy(void)
;//
;// The stack frame is assumed to look as follows:
;//
;//				  Entry Point(Task Name)				(High memory)
;//                               LR(R14)
;//                               R12
;//                               R11
;//                               R10
;//                               R9
;//                               R8
;//                               R7
;//                               R6
;//                               R5
;//                               R4
;//                               R3
;//                               R2
;//                               R1
;//                               Pdata R0 : argument
;// OSTCBHighRdy->OSTCBStkPtr --> CPSR								(Low memory)
;//
;// Note : OSStartHighRdy() MUST:
;//           a) Call OSTaskSwHook() then,
;//           b) Set OSRunning to TRUE,
;//           c) Switch to the highest priority task.
;//********************************************************************************************************** */
OSStartHighRdy
	MSR	CPSR_cxsf, #SVCMODE:OR:NOINT	;//switch to svcmode& disable irq&fiq
	BL	OSTaskSwHook
	LDR	R0, =OSRunning
	MOV	R1, #1
	STRB	R1, [R0]

	LDR R0, =OSTCBHighRdy
	LDR R0, [R0]
	LDR SP, [R0]

	LDMFD	SP!, {R0}
	MSR	SPSR_cxsf, R0
	LDMFD	SP!, {R0-R12, LR, PC}^
;//**********************************************************************************************************
;//                                PERFORM A CONTEXT SWITCH (From task level)
;//                                           void OSCtxSw(void)
;//
;// Note(s): 	  1) Upon entry: 
;//              	  OSTCBCur      points to the OS_TCB of the task to suspend
;//              	  OSTCBHighRdy  points to the OS_TCB of the task to resume
;//
;//          	   2) The stack frame of the task to suspend looks as follows:
;//                                                   
;//                                                     PC               (High memory)
;//				  				                        LR(R14)					
;//           					                        R12
;// 			                      			        R11
;//           		                			        R10
;//                   		           			 	    R9
;//                           		    			    R8
;//                               					    R7
;//                               					    R6
;//                               					    R5
;//                               					    R4
;//                               					    R3
;//                               					    R2
;//                               					    R1
;//                               					    R0
;// 					    OSTCBCur->OSTCBStkPtr ----> CPSR		     (Low memory)
;//
;//
;//          	   3) The stack frame of the task to resume looks as follows:
;
;//			  		  				                PC		     (High memory)
;//                                                 LR(R14)	
;//			           			                    R12
;//           		            			        R11
;//                   		        			    R10
;//                           		   			 	R9
;//                               					R8
;//                               					R7
;//			                               		 	R6
;//           		                    			R5
;//                   		            			R4
;//                           		    			R3
;//                               					R2
;//                               					R1
;//			                               			R0
;// 					OSTCBHighRdy->OSTCBStkPtr ---->	CPSR		      (Low memory)
;//*********************************************************************************************************/

OSCtxSw
	STMFD	SP!, {LR}
	STMFD	SP!, {R0-R12, LR}
	MRS	R0, CPSR
	STMFD	SP!, {R0}

	LDR	R0, =OSTCBCur
	LDR	R0, [R0]
	STR	SP, [R0]

	BL	OSTaskSwHook

	LDR	R0, =OSTCBHighRdy
	LDR	R1, =OSTCBCur
	LDR	R0, [R0]
	STR	R0, [R1]

	LDR	R0, =OSPrioHighRdy
	LDR	R1, =OSPrioCur
	LDRB	R0, [R0]
	STRB	R0, [R1]

	LDR	R0, =OSTCBCur
	LDR	R0, [R0]
	LDR	SP, [R0]

	LDMFD	SP!, {R0}
	MSR	SPSR_cxsf, R0
	LDMFD	SP!, {R0-R12, LR, PC}^

;//*********************************************************************************************************
;//                                PERFORM A CONTEXT SWITCH (From an ISR)
;//                                        void OSIntCtxSw(void)
;//
;// Description: 1) This code performs a context switch if a higher priority task has been made ready-to-run
;//               	during an ISR.
;//
;//          	   2) The stack frame of the task to suspend looks as follows:
;//
;//				  				                    PC					(High memory)
;//                                                 LR(R14)
;//           					                    R12
;// 			                      			    R11
;//           		                			    R10
;//                   		           			 	R9
;//                           		    			R8
;//                               					R7
;//                               					R6
;//                               					R5
;//                               					R4
;//                               					R3
;//                               					R2
;//                               					R1
;//                               					R0				
;// 					    OSTCBCur->OSTCBStkPtr ----> CPSR					(Low memory)
;//
;//
;//          	   3) The stack frame of the task to resume looks as follows:
;//
;//			  		  				                PC					(High memory)
;//                                                 LR(R14)	
;//			           			                    R12
;//           		            			        R11
;//                   		        			    R10
;//                           		   			 	R9
;//                               					R8
;//                               					R7
;//			                               			R6
;//           		                    			R5
;//                   		            			R4
;//                           		    			R3
;//                               					R2
;//                               					R1
;//			                               			R0
;// 					OSTCBHighRdy->OSTCBStkPtr ---->	CPSR					(Low memory)
;//*********************************************************************************************************/
OSIntCtxSw
	;//----------------------------------------------------------------------------------		
	;// Call OSTaskSwHook();
	;//----------------------------------------------------------------------------------	
	BL 		OSTaskSwHook
	
	;//----------------------------------------------------------------------------------			
	;// OSTCBCur = OSTCBHighRdy;
	;//----------------------------------------------------------------------------------		
	LDR		R0, =OSTCBHighRdy
	LDR		R1, =OSTCBCur
	LDR		R0, [R0]
	STR		R0, [R1]
	
	;//----------------------------------------------------------------------------------		
	;// OSPrioCur = OSPrioHighRdy;
	;//----------------------------------------------------------------------------------		
	LDR		R0, =OSPrioHighRdy
	LDR		R1, =OSPrioCur
	LDRB	R0, [R0]
	STRB	R0, [R1]
	
	;//----------------------------------------------------------------------------------		
	;// 		SP = OSTCBHighRdy->OSTCBStkPtr;
	;//----------------------------------------------------------------------------------		
	LDR		R0, =OSTCBHighRdy
	LDR		R0, [R0]
	LDR		SP, [R0]
	
	;//----------------------------------------------------------------------------------	
	;// Restore New Task context
	;//----------------------------------------------------------------------------------	
	LDMFD 	SP!, {R0}              ;POP CPSR
	MSR 	SPSR_cxsf, R0
	LDMFD 	SP!, {R0-R12, LR, PC}^	

;//*********************************************************************************************************
;//                                            TICK HANDLER
;//
;// Description:  
;//     This handles all the Timer0(INT_TIMER0) interrupt which is used to generate the uC/OS-II tick.
;//*********************************************************************************************************/
OSTickISR
	MOV	R5, LR

	LDR	R0, =SRCPND
	LDR	R1, [R0]
	STR	R1, [R0]

	LDR	R0, =INTPND
	LDR	R1, [R0]
	STR	R1, [R0]

	BL	OSTimeTick

	BX	R5
;//*********************************************************************************************************
;//                                PERFORM A CONTEXT SWITCH (From an ISR)
;//                                        void OSIntCtxSw(void)
;//
;// Description: 1) This code performs a context switch if a higher priority task has been made ready-to-run
;//               	during an ISR.
;//
;//          	   2) The stack frame of the task to suspend looks as follows:
;//
;//				  				                    PC					(High memory)
;//                                                 LR(R14)
;//           					                    R12
;// 			                      			    R11
;//           		                			    R10
;//                   		           			 	R9
;//                           		    			R8
;//                               					R7
;//                               					R6
;//                               					R5
;//                               					R4
;//                               					R3
;//                               					R2
;//                               					R1
;//                               					R0				
;// 					    OSTCBCur->OSTCBStkPtr ----> CPSR					(Low memory)
;//
;//
;//          	   3) The stack frame of the task to resume looks as follows:
;//
;//			  		  				                PC					(High memory)
;//                                                 LR(R14)	
;//			           			                    R12
;//           		            			        R11
;//                   		        			    R10
;//                           		   			 	R9
;//                               					R8
;//                               					R7
;//			                               			R6
;//           		                    			R5
;//                   		            			R4
;//                           		    			R3
;//                               					R2
;//                               					R1
;//			                               			R0
;// 					OSTCBHighRdy->OSTCBStkPtr ---->	CPSR					(Low memory)
;//*********************************************************************************************************/
;// OSIntCtxSw is same as half left of OSCtxSw, so we can use them as above

;//*********************************************************************************************************
;//                                PERFORM A IRQ
;//*********************************************************************************************************/
OS_CPU_IRQ_ISR
	STMFD	SP!, {R1-R3}
;//----------------------------------------------------------------------------
;//     R1--SP  (IRQ MODE)
;//	    R2--PC   (TASK POINTER) 
;//     R3--SPSR  (TASK CPSR)
;//----------------------------------------------------------------------------
	MOV	R1, SP
	ADD	SP, SP, #12
	SUB	R2, LR, #4
	MRS	R3, SPSR

	MSR	CPSR_cxsf, #SVCMODE:OR:NOINT
	;//take care! this sp is not the one before!svc mode's SP
	STMFD	SP!, {R2}
	STMFD	SP!, {R4-R12, LR}

	LDMFD	R1!, {R4-R6}
	STMFD	SP!, {R4-R6}
	STMFD	SP!, {R0}

	STMFD	SP!, {R3}

	;LDR	R0, =OSIntNesting
	;LDRB	R1, [R0]
	;ADD	R1, R1, #1
	;STRB	R1, [R0]
	
	;CMP	R1, #1
	;BNE	%F1

	LDR	R4, =OSTCBCur
	LDR	R5, [R4]
	LDR	SP, [R5]

	BL	OSIntEnter
 
 	MSR	CPSR_c, #IRQMODE:OR:NOINT
 	;/**
 	;LDR	R0, =INTOFFSET
 	;LDR	R0, [R0]

 	;LDR	R1, IRQIsrVect
 	;MOV	LR, PC
 	;LDR	PC, [R1, R0, LSL #2]
 	;*/
 	BL	irq_process

 	MSR	CPSR_c, #SVCMODE:OR:NOINT
 	BL	OSIntExit

 	LDMFD	SP!, {R4}
 	MSR	SPSR_cxsf, R4
	LDMFD	SP!, {R0-R12, LR, PC}^

;//*********************************************************************************************************
;//                                   CRITICAL SECTION METHOD 3 FUNCTIONS
;//
;// Description: Disable/Enable interrupts by preserving the state of interrupts.  Generally speaking you
;//              would store the state of the interrupt disable flag in the local variable 'cpu_sr' and then
;//              disable interrupts.  'cpu_sr' is allocated in all of uC/OS-II''s functions that need to 
;//              disable interrupts.  You would restore the interrupt disable state by copying back 'cpu_sr'
;//              into the CPU''s status register.
;//
;// Prototypes : OS_CPU_SR  OSCPUSaveSR(void);
;//              void       OSCPURestoreSR(OS_CPU_SR cpu_sr);
;//
;//
;// Note(s)    : 1) These functions are used in general like this:
;//
;//                 void Task (void *p_arg)
;//                 {
;//                 #if OS_CRITICAL_METHOD == 3          /* Allocate storage for CPU status register */
;//                     OS_CPU_SR  cpu_sr; //OS_ALLOC_SR();
;//                 #endif
;//
;//                          :
;//                          :
;//                     OS_ENTER_CRITICAL();             /* cpu_sr = OSCPUSaveSR();                */
;//                          :
;//                          :
;//                     OS_EXIT_CRITICAL();              /* OSCPURestoreSR(cpu_sr);                */
;//                          :
;//                          :
;//                 }
;//
;//              2) OSCPUSaveSR() is implemented as recommended by Atmel''s application note:
;//
;//                    "Disabling Interrupts at Processor Level"
;//*********************************************************************************************************
OS_CPU_SR_Save

	MRS		R0, CPSR
	ORR		R1, R0, #0xC0
	MSR		CPSR_c, R1
	MRS		R1, CPSR
	AND		R1, R1, #0xC0
	CMP		R1, #0xC0
	BNE		OS_CPU_SR_Save
	BX		lr
	
OS_CPU_SR_Restore
	MSR		CPSR_c, R0
	BX		LR

	END
	


